#include "stm32f4xx.h"
#include "bsp.h"
#include "icm40607.h"
#include "communication_interface.h"
#include "fir_filter.h"
#include "app_sensor.h"
#include "drv_can.h"
#include "ist8310.h"
#include "math.h"
#include "monitor.h"
#include "rtthread.h"

#define CALC_UPDATE_SPEED					1
#define FILTER_400_50_TAP       	4
#define FILTER_2K_100_TAP       	4

#define FILTER_GYROZ_TAP 					4

#define MATH_PI 3.1415f


//FIR filter params
const float c_fFactor_400_50_4[FILTER_400_50_TAP] = {0.223, 0.277, 0.277, 0.223};
const float c_fFactor_2K_100_4[FILTER_2K_100_TAP] = {0.2459, 0.2541, 0.2541, 0.2459};
const float c_fFactor_GyroZ[FILTER_GYROZ_TAP] = {0.4,0.3,0.2,0.1};
//acc 
static float s_fAccxFilter_Delay[FILTER_400_50_TAP];
static float s_fAccyFilter_Delay[FILTER_400_50_TAP];
static float s_fAcczFilter_Delay[FILTER_400_50_TAP];

static FIR_FILTER_Float_DEF s_tAccxFilter = {s_fAccxFilter_Delay, c_fFactor_400_50_4, FILTER_400_50_TAP};
static FIR_FILTER_Float_DEF s_tAccyFilter = {s_fAccyFilter_Delay, c_fFactor_400_50_4, FILTER_400_50_TAP};
static FIR_FILTER_Float_DEF s_tAcczFilter = {s_fAcczFilter_Delay, c_fFactor_400_50_4, FILTER_400_50_TAP};

//gyro
static float s_fGyroxFilter_Delay[FILTER_2K_100_TAP];
static float s_fGyroyFilter_Delay[FILTER_2K_100_TAP];
static float s_fGyrozFilter_Delay[FILTER_2K_100_TAP];

static FIR_FILTER_Float_DEF s_tGyroxFilter = {s_fGyroxFilter_Delay, c_fFactor_2K_100_4, FILTER_2K_100_TAP};
static FIR_FILTER_Float_DEF s_tGyroyFilter = {s_fGyroyFilter_Delay, c_fFactor_2K_100_4, FILTER_2K_100_TAP};
static FIR_FILTER_Float_DEF s_tGyrozFilter = {s_fGyrozFilter_Delay, c_fFactor_2K_100_4, FILTER_2K_100_TAP};  


//sensor lib update
SensorUpdateParasInfo s_slibSystemSensorCal = {0};


//cali info
SysAppInfo g_tSysAppInfo = {0};
SYSTEM_STATUS g_tSysStatus = {0};         
IMU_ConfigTypeDef s_IMU_config;
static IMU_SensorRawDataTypeDef g_IMU_sensor_data;
static SensorZeros s_sensorZeros = {0.0f,0.0f,0.0f,
																		-0.84f,-0.40f,1.47f,
																		0.0f,0.0f,0.0f};
	
/*********shanghai daiwu 
																		{0.0,	0.0,	0.0,//accel x y z
																		0.05, 0.0,	0.43,//gyro  x y z
																		0.0,	0.0,	0.0};//compass x y z
*********/

extern MAGREG_TypeDef Mag_sensor;

extern rt_sem_t rt_sem_sensor ;
extern rt_sem_t rt_sem_magnect ;
extern rt_sem_t rt_sem_monitor ;

//u32 sensortask_cnt = 0;

rt_err_t result;
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SensorTask_entry(void* parameter)
{
//		rt_sem_sensor = rt_sem_create("dsem1", 0, RT_IPC_FLAG_PRIO);//create sensortask sem
//		rt_sem_magnect = rt_sem_create("dsem2", 0, RT_IPC_FLAG_PRIO);//create magnecttask sem
//		rt_sem_monitor = rt_sem_create("dsem3", 0, RT_IPC_FLAG_PRIO);//create monitortask sem

	SensorAppInit();
	while(1)
	{
		
		CalculateMagAngle(&(g_tSysStatus.sensor_info));
		
		UpdateYawAngle(&(g_tSysStatus.sensor_info));

		SensorAppUpdateHandle();
//		sensortask_cnt++;
		
		result = rt_sem_take(rt_sem_sensor,RT_WAITING_FOREVER);
		

	}
}

/**
  * @name						SensorAppInit
  * @brief					pitch roll yawq datas inital handlers from IMU data 
  * @param  				none
  * @retval 				none 
  */
void SensorAppInit(void)
{
	//Reset sonsor system
	ResetSystermAll();
	
	/* set app flatform */
	SetAppPlatform(PLATFORM_SKIBOARD);
	
	//set filter update freq  set to 1k
	//task freq = kalman freq = 4096 / param;
	SetSystermFilterTime(4);

	//Init filter
	FIR_Filter_float_Clear(&s_tAccxFilter);
	FIR_Filter_float_Clear(&s_tAccyFilter);
	FIR_Filter_float_Clear(&s_tAcczFilter);
		
	FIR_Filter_float_Clear(&s_tGyroxFilter);
	FIR_Filter_float_Clear(&s_tGyroyFilter);
	FIR_Filter_float_Clear(&s_tGyrozFilter);	
	
	SetSystermFilterKg(3);
}


/**
  * @name					SensorAppUpdateHandle
  * @brief				
  * @param  
  * @retval 
  */
void SensorAppUpdateHandle(void)
{
		float raw_acce_x, raw_acce_y, raw_acce_z;
		float raw_gyro_x, raw_gyro_y, raw_gyro_z;	
		float raw_mag_x,  raw_mag_y,	raw_mag_z;
		static float fuse_yaw_angle;
	

		ReadIMU_AllSensors( &g_IMU_sensor_data);
	

			/* Gyro coordinate transfer, apply to sensor_lib coordinate */
		raw_gyro_x = (float)+g_IMU_sensor_data.gyro_x / s_IMU_config.gyro_LSB;
		raw_gyro_y = (float)+g_IMU_sensor_data.gyro_y / s_IMU_config.gyro_LSB;
		raw_gyro_z = (float)+g_IMU_sensor_data.gyro_z / s_IMU_config.gyro_LSB;
		
		/* Gyro FIR filter and cail zero */				
		g_tSysStatus.sensor_info.gyro_x_raw = FIR_Filter_float(&s_tGyroxFilter, raw_gyro_x) - s_sensorZeros.gyro_x_zero;
		g_tSysStatus.sensor_info.gyro_y_raw = FIR_Filter_float(&s_tGyroyFilter, raw_gyro_y) - s_sensorZeros.gyro_y_zero;
		g_tSysStatus.sensor_info.gyro_z_raw = FIR_Filter_float(&s_tGyrozFilter, raw_gyro_z) - s_sensorZeros.gyro_z_zero;
		
		/* Accel coordinate transfer, apply to sensor_lib coordinate */
		raw_acce_x = (float)+g_IMU_sensor_data.accel_y / s_IMU_config.accel_LSB;
		raw_acce_y = (float)-g_IMU_sensor_data.accel_x / s_IMU_config.accel_LSB;
		raw_acce_z = (float)-g_IMU_sensor_data.accel_z / s_IMU_config.accel_LSB;
		
		/* Accel FIR filter and cail zero */
		g_tSysStatus.sensor_info.acce_x_raw = FIR_Filter_float(&s_tAccxFilter, raw_acce_x) - s_sensorZeros.accel_x_zero;
		g_tSysStatus.sensor_info.acce_y_raw = FIR_Filter_float(&s_tAccyFilter, raw_acce_y) - s_sensorZeros.accel_y_zero;
		g_tSysStatus.sensor_info.acce_z_raw = FIR_Filter_float(&s_tAcczFilter, raw_acce_z) - s_sensorZeros.accel_z_zero;
		
		/* Mag coordinate transfer, apply to sensor_lib coordinate */
		raw_mag_x = (float)Mag_sensor.x_axis / 125;
		raw_mag_y = (float)Mag_sensor.y_axis / 125;
		raw_mag_z = (float)Mag_sensor.z_axis / 125;
		
		g_tSysStatus.sensor_info.magnet_x_raw = (raw_mag_x + s_sensorZeros.mag_x_zero);
		g_tSysStatus.sensor_info.magnet_y_raw = (raw_mag_y + s_sensorZeros.mag_y_zero) * 1.1;
		g_tSysStatus.sensor_info.magnet_z_raw = (raw_mag_z + s_sensorZeros.mag_z_zero);
		
		/********************** Calculate IMU sensor temperature *****************/
		g_tSysStatus.sys_temprature = (s16)((float)( g_IMU_sensor_data.temp ) / 132.48) + 25;
		
		//????
		s_slibSystemSensorCal.sensor_info.gyro_x_raw = (s32)(g_tSysStatus.sensor_info.gyro_x_raw * 4096);
		s_slibSystemSensorCal.sensor_info.gyro_y_raw = (s32)(g_tSysStatus.sensor_info.gyro_y_raw * 4096);
		s_slibSystemSensorCal.sensor_info.gyro_z_raw = (s32)(g_tSysStatus.sensor_info.gyro_z_raw * 4096);
		
		s_slibSystemSensorCal.sensor_info.acce_x_raw = (s32)(g_tSysStatus.sensor_info.acce_x_raw * 4096);
		s_slibSystemSensorCal.sensor_info.acce_y_raw = (s32)(g_tSysStatus.sensor_info.acce_y_raw * 4096);
		s_slibSystemSensorCal.sensor_info.acce_z_raw = (s32)(g_tSysStatus.sensor_info.acce_z_raw * 4096);

		s_slibSystemSensorCal.sensor_info.PitchAngle_zero = g_tSysAppInfo.usPitchZero * 4096 / 65536;
		s_slibSystemSensorCal.fused_system = DOUBLE_AXIS;
		s_slibSystemSensorCal.balance_flag = g_tSysStatus.balance_flag;
		
		//update pose 
		AppUpdateSensor(&s_slibSystemSensorCal);
			
		//???
		g_tSysStatus.sensor_info.fused_pitch_angle = (float)(s_slibSystemSensorCal.sensor_info.fused_pitch_angle / 4096.0); 
		g_tSysStatus.sensor_info.fused_roll_angle = (float)(s_slibSystemSensorCal.sensor_info.fused_roll_angle / 4096.0);
//		g_tSysStatus.sensor_info.fused_yaw_angle = (float)(s_slibSystemSensorCal.sensor_info.fused_yaw_angle / 4096.0);
		
		g_tSysStatus.sensor_info.static_pitch_angle = (float)(s_slibSystemSensorCal.sensor_info.static_pitch_angle / 4096.0); 
		g_tSysStatus.sensor_info.static_roll_angle = (float)(s_slibSystemSensorCal.sensor_info.static_roll_angle / 4096.0);
//		g_tSysStatus.sensor_info.static_yaw_angle = (float)(s_slibSystemSensorCal.sensor_info.static_yaw_angle / 4096.0);
		
		g_tSysStatus.sensor_info.pitch_angle_rate = (float)(s_slibSystemSensorCal.sensor_info.pitch_angle_rate / 4096.0);
		g_tSysStatus.sensor_info.roll_angle_rate = (float)(s_slibSystemSensorCal.sensor_info.roll_angle_rate / 4096.0);


}


/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
const float DetaT = 0.001;
const float Yaw_Kg = 0.3;
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */

void UpdateYawAngle(App_SensorInfo *str )
{ 
	float staticyaw_angle = 0;
	float fuse_yaw_angle = 0;
	float deta_angle = 0;
	float gyro_z = 0;
	
	fuse_yaw_angle = str->fused_yaw_angle;
	staticyaw_angle = str->static_yaw_angle;
	gyro_z = str->gyro_z_raw;
	
	if((staticyaw_angle - fuse_yaw_angle) > 180)
		staticyaw_angle -= 360;
	if((staticyaw_angle - fuse_yaw_angle ) < -180)
		staticyaw_angle += 360;	
	
	deta_angle =  gyro_z + Yaw_Kg * (staticyaw_angle - fuse_yaw_angle) ;
	deta_angle /= 1000;
	
	fuse_yaw_angle  += deta_angle;
	
	if( fuse_yaw_angle > 180 )
			fuse_yaw_angle -= 360;
	if( fuse_yaw_angle < -180 )
		fuse_yaw_angle += 360;
	
	str->fused_yaw_angle = fuse_yaw_angle;
}
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void CalculateMagAngle(App_SensorInfo *str )	
{
	float Mag_angle_rad,x,y;
		
		x = str->magnet_x_raw;
		y = str->magnet_y_raw;
	
				/****************************** cal magnet sensor raw yaw angle **************************************/
		if			((x <= 0 )&&(y <=0))
			Mag_angle_rad = atanf((float)y / (float)x);
		else if ((x >= 0 )&&(y <=0))
			Mag_angle_rad = atanf((float)y / (float)x) + MATH_PI ;
		else if ((x >= 0 )&&(y >=0))
			Mag_angle_rad = atanf((float)y / (float)x) - MATH_PI ;
		else 
			Mag_angle_rad = atanf((float)y / (float)x)  ;
		
		if((Mag_angle_rad <= 180)&&( Mag_angle_rad >= -180))
				str->static_yaw_angle =  (float)(180 * Mag_angle_rad / MATH_PI)  ;
	

}

 u16 MagTask_cnt = 0;
 void MagnectTask_entry(void* parameter)
 {
	 while(1)
	 {
		Read_MAG3axis( &Mag_sensor);
//		MagTask_cnt ++; 		 
		rt_sem_take(rt_sem_magnect,RT_WAITING_FOREVER);

//		rt_thread_delay(10);		 
	 }
 }



	


/**********************************************  END OF FILE sensor.c *****************************************************************/
